In [1]:

    

from miscpy.utils.sympyhelpers import *
init_printing()


    

In [2]:

    

th,ph,psi,thd,phd,psid,thdd,phdd,psidd,rho,R = symbols('theta,phi,psi,thetadot,phidot,psidot,thetaddot,phiddot,psiddot,rho,R')


    

In [3]:

    

r_dm_O = Matrix([rho*cos(th)*sin(ph),rho*sin(th)*sin(ph), rho*cos(ph)]);r_dm_O


    

    
Out[3]:





$$\left[\begin{matrix}\rho \sin{\left (\phi \right )} \cos{\left (\theta \right )}\\\rho \sin{\left (\phi \right )} \sin{\left (\theta \right )}\\\rho \cos{\left (\phi \right )}\end{matrix}\right]$$

In [4]:

    

integrate(integrate(integrate(r_dm_O*rho**2*sin(ph),(rho,0,R)),(th,0,2*pi)),(ph,0,pi/2))


    

    
Out[4]:





$$\left[\begin{matrix}0\\0\\\frac{\pi R^{4}}{4}\end{matrix}\right]$$

In [5]:

    

integrate(rho**2*sin(ph),(rho,0,R),(th,0,2*pi),(ph,0,pi/2))


    

    
Out[5]:





$$\frac{2 \pi}{3} R^{3}$$

In [6]:

    

g,t,h,m,l,thdd,phdd,psidd = symbols('g,t,h,m,l,thetaddot,phiddot,psiddot')


    

In [7]:

    

iWs = Matrix([-thd*sin(ph),phd,thd*cos(ph)])
h_O = Matrix([-m*l**2*thd*sin(ph),m*l**2*phd,h]); h_O,iWs


    

    
Out[7]:





$$\left ( \left[\begin{matrix}- l^{2} m \dot{\theta} \sin{\left (\phi \right )}\\l^{2} m \dot{\phi}\\h\end{matrix}\right], \quad \left[\begin{matrix}- \dot{\theta} \sin{\left (\phi \right )}\\\dot{\phi}\\\dot{\theta} \cos{\left (\phi \right )}\end{matrix}\right]\right )$$

In [8]:

    

dh_O = difftotalmat(h_O,t,{th:thd,thd:thdd,ph:phd,phd:phdd,psi:psid,psid:psidd}) + h_O[0]*iWs.cross(Matrix([1,0,0])) +h_O[1]*iWs.cross(Matrix([0,1,0])) + h_O[2]*iWs.cross(Matrix([0,0,1]))
dh_O


    

    
Out[8]:





$$\left[\begin{matrix}h \dot{\phi} - 2 l^{2} m \dot{\phi} \dot{\theta} \cos{\left (\phi \right )} - l^{2} m \ddot{\theta} \sin{\left (\phi \right )}\\h \dot{\theta} \sin{\left (\phi \right )} + l^{2} m \ddot{\phi} - l^{2} m \dot{\theta}^{2} \sin{\left (\phi \right )} \cos{\left (\phi \right )}\\0\end{matrix}\right]$$

In [9]:

    

sol1 = solve(dh_O - Matrix([0,m*g*l*sin(ph),0]),(thdd,phdd)); sol1


    

    
Out[9]:





$$\left \{ \ddot{\phi} : \frac{\sin{\left (\phi \right )}}{l^{2} m} \left(g l m - h \dot{\theta} + l^{2} m \dot{\theta}^{2} \cos{\left (\phi \right )}\right), \quad \ddot{\theta} : \frac{\dot{\phi} \left(h - 2 l^{2} m \dot{\theta} \cos{\left (\phi \right )}\right)}{l^{2} m \sin{\left (\phi \right )}}\right \}$$

In [10]:

    

solve(sol1[phdd],ph)


    

    
Out[10]:





$$\left [ 0, \quad \pi, \quad - \operatorname{acos}{\left (- \frac{\frac{g}{\dot{\theta}} - \frac{h}{l m}}{l \dot{\theta}} \right )} + 2 \pi, \quad \operatorname{acos}{\left (- \frac{g}{l \dot{\theta}^{2}} + \frac{h}{l^{2} m \dot{\theta}} \right )}\right ]$$

In [ ]:

    




    

In [11]:

    

r,z,h = symbols('r,z,h')


    

In [12]:

    

r_dm_G = Matrix([r*cos(th),r*sin(th), z]);r_dm_G


    

    
Out[12]:





$$\left[\begin{matrix}r \cos{\left (\theta \right )}\\r \sin{\left (\theta \right )}\\z\end{matrix}\right]$$

In [13]:

    

simplify(r_dm_G.transpose()*r_dm_G)


    

    
Out[13]:





$$\left[\begin{matrix}r^{2} + z^{2}\end{matrix}\right]$$

In [14]:

    

r_dm_G*r_dm_G.transpose()


    

    
Out[14]:





$$\left[\begin{matrix}r^{2} \cos^{2}{\left (\theta \right )} & r^{2} \sin{\left (\theta \right )} \cos{\left (\theta \right )} & r z \cos{\left (\theta \right )}\\r^{2} \sin{\left (\theta \right )} \cos{\left (\theta \right )} & r^{2} \sin^{2}{\left (\theta \right )} & r z \sin{\left (\theta \right )}\\r z \cos{\left (\theta \right )} & r z \sin{\left (\theta \right )} & z^{2}\end{matrix}\right]$$

In [15]:

    

kern1 = ((simplify(r_dm_G.transpose()*r_dm_G))[0]*eye(3) - r_dm_G*r_dm_G.transpose())
kern1


    

    
Out[15]:





$$\left[\begin{matrix}- r^{2} \cos^{2}{\left (\theta \right )} + r^{2} + z^{2} & - r^{2} \sin{\left (\theta \right )} \cos{\left (\theta \right )} & - r z \cos{\left (\theta \right )}\\- r^{2} \sin{\left (\theta \right )} \cos{\left (\theta \right )} & - r^{2} \sin^{2}{\left (\theta \right )} + r^{2} + z^{2} & - r z \sin{\left (\theta \right )}\\- r z \cos{\left (\theta \right )} & - r z \sin{\left (\theta \right )} & r^{2}\end{matrix}\right]$$

In [18]:

    

simplify(integrate(integrate(integrate(kern1*r,(r,0,R)),(th,0,2*pi)),(z,-h/2,h/2))*m/pi/R**2/h)


    

    
Out[18]:





$$\left[\begin{matrix}\frac{m}{12} \left(3 R^{2} + h^{2}\right) & 0 & 0\\0 & \frac{m}{12} \left(3 R^{2} + h^{2}\right) & 0\\0 & 0 & \frac{R^{2} m}{2}\end{matrix}\right]$$

In [ ]:

    




    

In [19]:

    

a1,a2,a3,b1,b2,b3 = symbols('a_1,a_2,a_3,b_1,b_2,b_3')


    

In [20]:

    

a = Matrix([a1,a2,a3])
b = Matrix([b1,b2,b3])
a,b


    

    
Out[20]:





$$\left ( \left[\begin{matrix}a_{1}\\a_{2}\\a_{3}\end{matrix}\right], \quad \left[\begin{matrix}b_{1}\\b_{2}\\b_{3}\end{matrix}\right]\right )$$

In [21]:

    

a.cross(b)


    

    
Out[21]:





$$\left[\begin{matrix}a_{2} b_{3} - a_{3} b_{2}\\- a_{1} b_{3} + a_{3} b_{1}\\a_{1} b_{2} - a_{2} b_{1}\end{matrix}\right]$$

In [22]:

    

at = Matrix([[0,-a3,a2],[a3,0,-a1],[-a2,a1,0]]); at


    

    
Out[22]:





$$\left[\begin{matrix}0 & - a_{3} & a_{2}\\a_{3} & 0 & - a_{1}\\- a_{2} & a_{1} & 0\end{matrix}\right]$$

In [23]:

    

at*b


    

    
Out[23]:





$$\left[\begin{matrix}a_{2} b_{3} - a_{3} b_{2}\\- a_{1} b_{3} + a_{3} b_{1}\\a_{1} b_{2} - a_{2} b_{1}\end{matrix}\right]$$

In [ ]:

    




    

In [24]:

    

I1,I2,I3,O,t,M1 = symbols('I_1,I_2,I_3,Omega,t,M_1')


    

In [25]:

    

iWc = Matrix([thd,psid*sin(th)+O,psid*cos(th)])
iWb = Matrix([thd,psid*sin(th),psid*cos(th)])
iWb,iWc


    

    
Out[25]:





$$\left ( \left[\begin{matrix}\dot{\theta}\\\dot{\psi} \sin{\left (\theta \right )}\\\dot{\psi} \cos{\left (\theta \right )}\end{matrix}\right], \quad \left[\begin{matrix}\dot{\theta}\\\Omega + \dot{\psi} \sin{\left (\theta \right )}\\\dot{\psi} \cos{\left (\theta \right )}\end{matrix}\right]\right )$$

In [26]:

    

I_G = diag(I1,I2,I1); I_G


    

    
Out[26]:





$$\left[\begin{matrix}I_{1} & 0 & 0\\0 & I_{2} & 0\\0 & 0 & I_{1}\end{matrix}\right]$$

In [27]:

    

h_G = I_G*iWc; h_G


    

    
Out[27]:





$$\left[\begin{matrix}I_{1} \dot{\theta}\\I_{2} \left(\Omega + \dot{\psi} \sin{\left (\theta \right )}\right)\\I_{1} \dot{\psi} \cos{\left (\theta \right )}\end{matrix}\right]$$

In [28]:

    

dh_G= difftotalmat(h_G,t,{th:thd,thd:thdd,psi:psid,psid:psidd}); dh_G


    

    
Out[28]:





$$\left[\begin{matrix}I_{1} \ddot{\theta}\\I_{2} \left(\ddot{\psi} \sin{\left (\theta \right )} + \dot{\psi} \dot{\theta} \cos{\left (\theta \right )}\right)\\I_{1} \ddot{\psi} \cos{\left (\theta \right )} - I_{1} \dot{\psi} \dot{\theta} \sin{\left (\theta \right )}\end{matrix}\right]$$

In [29]:

    

t2 = iWb.cross(h_G); t2


    

    
Out[29]:





$$\left[\begin{matrix}I_{1} \dot{\psi}^{2} \sin{\left (\theta \right )} \cos{\left (\theta \right )} - I_{2} \dot{\psi} \left(\Omega + \dot{\psi} \sin{\left (\theta \right )}\right) \cos{\left (\theta \right )}\\0\\- I_{1} \dot{\psi} \dot{\theta} \sin{\left (\theta \right )} + I_{2} \dot{\theta} \left(\Omega + \dot{\psi} \sin{\left (\theta \right )}\right)\end{matrix}\right]$$

In [30]:

    

dh_G + t2,Matrix([-M1,0,0])


    

    
Out[30]:





$$\left ( \left[\begin{matrix}I_{1} \dot{\psi}^{2} \sin{\left (\theta \right )} \cos{\left (\theta \right )} + I_{1} \ddot{\theta} - I_{2} \dot{\psi} \left(\Omega + \dot{\psi} \sin{\left (\theta \right )}\right) \cos{\left (\theta \right )}\\I_{2} \left(\ddot{\psi} \sin{\left (\theta \right )} + \dot{\psi} \dot{\theta} \cos{\left (\theta \right )}\right)\\I_{1} \ddot{\psi} \cos{\left (\theta \right )} - 2 I_{1} \dot{\psi} \dot{\theta} \sin{\left (\theta \right )} + I_{2} \dot{\theta} \left(\Omega + \dot{\psi} \sin{\left (\theta \right )}\right)\end{matrix}\right], \quad \left[\begin{matrix}- M_{1}\\0\\0\end{matrix}\right]\right )$$

In [31]:

    

dh_G + t2 - Matrix([-M1,0,0])


    

    
Out[31]:





$$\left[\begin{matrix}I_{1} \dot{\psi}^{2} \sin{\left (\theta \right )} \cos{\left (\theta \right )} + I_{1} \ddot{\theta} - I_{2} \dot{\psi} \left(\Omega + \dot{\psi} \sin{\left (\theta \right )}\right) \cos{\left (\theta \right )} + M_{1}\\I_{2} \left(\ddot{\psi} \sin{\left (\theta \right )} + \dot{\psi} \dot{\theta} \cos{\left (\theta \right )}\right)\\I_{1} \ddot{\psi} \cos{\left (\theta \right )} - 2 I_{1} \dot{\psi} \dot{\theta} \sin{\left (\theta \right )} + I_{2} \dot{\theta} \left(\Omega + \dot{\psi} \sin{\left (\theta \right )}\right)\end{matrix}\right]$$

In [ ]:

    




    

In [32]:

    

I1,I2,I3,w1,w2,w3,w1d,w2d,w3d,M1,M2,M3 = symbols('I_1,I_2,I_3,omega_1,omega_2,omega_3,omegadot_1,omegadot_2,omegadot_3,M_1,M_2,M_3')


    

In [33]:

    

I_G = diag(I1,I2,I3)
w = Matrix([w1,w2,w3])
dw = difftotalmat(w,t,{w1:w1d,w2:w2d,w3:w3d})
wt = Matrix([[0,-w3,w2],[w3,0,-w1],[-w2,w1,0]])
M = Matrix([M1,M2,M3])
(I_G,w,dw,wt,M)


    

    
Out[33]:





$$\left ( \left[\begin{matrix}I_{1} & 0 & 0\\0 & I_{2} & 0\\0 & 0 & I_{3}\end{matrix}\right], \quad \left[\begin{matrix}\omega_{1}\\\omega_{2}\\\omega_{3}\end{matrix}\right], \quad \left[\begin{matrix}\dot{\omega}_{1}\\\dot{\omega}_{2}\\\dot{\omega}_{3}\end{matrix}\right], \quad \left[\begin{matrix}0 & - \omega_{3} & \omega_{2}\\\omega_{3} & 0 & - \omega_{1}\\- \omega_{2} & \omega_{1} & 0\end{matrix}\right], \quad \left[\begin{matrix}M_{1}\\M_{2}\\M_{3}\end{matrix}\right]\right )$$

In [ ]: